/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "coordinateinstructionlogic.h"
#include "GeneralDefine.h"
#include "programengine.h"
#include "messagelog.h"

CoordinateInstructionLogic::CoordinateInstructionLogic(int robotIdIn,
                                                       RobotIo* ioControllerIn, ProgramEngine* programEngineIn,
                                                       McControllerThread *programMcControllerIn, RegData *regDataIn)
{
    regData=regDataIn;
    robotId=robotIdIn;
    ioController=ioControllerIn;
    programEngine=programEngineIn;
    programMcController=programMcControllerIn;
}


E_PROGRAM_LOOP_RESULT CoordinateInstructionLogic::startCoorDinate(int programNum,
                                                                        CoorDinateInstruction coorInstruct)
{
    programEngine->updateProgramSetRegister();

    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;
//    qDebug() << "****coorType====="<<coorInstruct.coorType<<"&&&number&&&&"<<coorInstruct.back_val<<coorInstruct.backType<<endl;

    int coordinateNum = coorInstruct.back_val;

    switch(coorInstruct.backType)
    {
        case INDIRECT:
        {
            double tmpValue;
            if(1!=regData->getRRegValue(coordinateNum - 1,tmpValue))
            {
                addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3017,robotId);
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            coordinateNum=tmpValue;
        }
            break;
        case DIRECT:
        {

        }
        break;
    }

//    qDebug() << "====coordinateNum====="<<coordinateNum<<"===coorInstruct.back_val===="<<coorInstruct.back_val<<"\n\n";

    if( UFRAME_TYPE == coorInstruct.coorType)
    {
        programMcController->mcControllerInterpreter->activateUserCoordinate(coordinateNum);
    }
    else if(UTOOL_TYPE == coorInstruct.coorType )
    {
        programMcController->mcControllerInterpreter->activateToolCoordinate(coordinateNum);
    }
    else if(UFRAME_PR_TYPE == coorInstruct.coorType)
    {
        int ufIndex = coorInstruct.front_val;
        int prVal = coorInstruct.back_val - 1;
        PointPro pointPro;
        if(1!=regData->getPrValue(prVal,pointPro))
        {
            addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3017,robotId);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }


        UserCoordinateInfo userCoordinateInfo;
        userCoordinateInfo.index = ufIndex;
        userCoordinateInfo.method = ENUM_USER_XYZWPR;
        PointPro point = programMcController->mcControllerInterpreter->pointCoordinateToCartesian(pointPro);
        userCoordinateInfo.xyzwpr = QVector<double>::fromStdVector( point.positionValue);
//        userCoordinateInfo.xyzwpr.resize(pointPro.positionValue.size());
//        for(int i = 0; i < userCoordinateInfo.xyzwpr.size();i++)
//        {
//            userCoordinateInfo.xyzwpr[i] = pointPro.positionValue[i];
//        }

        programMcController->mcControllerInterpreter->setUserCoordinate(userCoordinateInfo);
    }
    else if(UTOOL_PR_TYPE == coorInstruct.coorType)
    {
        int utIndex = coorInstruct.front_val;
        int prVal = coorInstruct.back_val-1;
        PointPro pointPro;
        if(1!=regData->getPrValue(prVal,pointPro))
        {
            addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3017,robotId);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        ToolCoordinateInfo toolCoordinateInfo;
        toolCoordinateInfo.index = utIndex;
        toolCoordinateInfo.method = ENUM_TOOL_XYZWPR;
        PointPro point = programMcController->mcControllerInterpreter->pointCoordinateToCartesian(pointPro);
        toolCoordinateInfo.xyzwpr = QVector<double>::fromStdVector( point.positionValue);
        programMcController->mcControllerInterpreter->setToolCoordinate(toolCoordinateInfo);
        qDebug()<<"ProgramEngine::startCoorDinate===="<<toolCoordinateInfo.xyzwpr<<"\n\n\n";
    }


    programEngine->calculateNextLineToRun(programNum);


    endFlag = programEngine->isTheLastInstruction(programNum);
    return endFlag;
}


void CoordinateInstructionLogic::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 3017:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",寄存器指令错误，寄存器的范围超过限定的最大范围，请修改");
    }
        break;

    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}
